/*============================================================================*/
/*                                                                            */
/* cordet.c                                                                   */
/*                                                                            */
/* This is an implementation of the algorithm for detecting the corners       */
/* in digital images that was developed at VSB-Technical University           */
/* of Ostrava. The theory of the algorithm and other details are available at */
/*                                                                            */
/* http://www.cs.vsb.cz/sojka/cordet/presentation.html                        */
/*                                                                            */
/* If you want just only to use the algorithm, you will only call the         */
/* function "detectCorners". The declaration of that function can be found    */
/* in the file "cordet.h". You should understand what the particular          */
/* parameters of the function exactly mean. You will find the detailed        */
/* explanation as well as practical advice how to set them in the file        */
/* "cordet.h". You can also have a look at the test program in "cortest.c"    */
/* and its corresponding script files to obtain practical examples.           */
/*                                                                            */
/* If you need some further assistance or if you have some comments, please   */
/* be so kind and contact me at "eduard.sojka@vsb.cz".                        */
/*                                                                            */
/* This version of implementation is from 26 May 2003. Please, contact me     */
/* if you read this message substantially later. I may be able to provide     */
/* you with a newer version.                                                  */
/*                                                                            */
/* (c) Eduard Sojka, 2003                                                     */
/*                                                                            */
/*============================================================================*/
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include "cordet.h"

#define PIf      3.1415926f
#define TWO_PIf  6.2831853f
#define HALF_PIf 1.5707963f
#define MIN_APPAR -999.9f

#define SMALL_PROBABILITY 0.05f    /* The points whose Psg(X) is less than    */
                              /* this value are excluded from further process-*/
                              /* ing since they have almost no influence.     */
#define PD_TABLE_SCALE 250    /* The function pd() is tabulated with the step */
                              /* 1.0/this value.                              */
#define SMALL_NUM_PIXELS 3    /* Minimal number of points X that are required */
                              /* for carrying out the measurement at Q.       */
#define ANGLE_RAMP_WIDTH 0.35f/* Theoretically, the angle between the corner  */
                              /* axis and the directions of the gradient at   */
                              /* points in the corner area cannot be greater  */
                              /* than pi/2. Practically, the algorithm does   */
                              /* not use this theoretical step function and   */
                              /* uses a ramp function instead. The width of   */
                              /* the ramp is defined by this value.           */

/*----------------------------------------------------------------------------*/
/* Some globals follow that are mostly used to save the parameters.           */
/* If you want just only to try or use the detector do not look at them.      */
/* If you want to study the program, I recommend reading the theory first     */
/* since the identifiers usually correspond with the notation introduced      */
/* there.                                                                     */
/*                                                                            */
float *image;                 /* pointer to the input image                   */
int    xImageSize;            /* width of the input image                     */
int    yImageSize;            /* height of the input image                    */
int    numImagePixs;          /* number of pixels in the image                */
int    halfPsgMaskSize;       /* the overall mask size is 2*halfMaskSize + 1  */
int    PsgMaskSize;           /* the overall mask size, mask = neighbourhood  */
int    PsgMaskCenter;         /* index of centerpoint in mask                 */
int    numPsgMaskPixs;        /* number of pixels in Psg mask                 */
float  noiseGradSizeThresh;   /* threshold for the gradient size              */
float  meanGradSizeThresh;    /* threshold for the mean gradient size over    */
float  inertiaRadiusThresh;   /* threshold for the corner area size           */
float  halfGradSizeThresh;    /* one half of the value above                  */
float  tenthGradSizeThresh;   /* one tenth of gradSizeThresh                  */
float  angleThresh;           /* threshold for the break angle at the corner  */
float  halfAngleThresh;       /* simply the half of the above value           */
float  apparenceThresh;       /* threshold for the apparence of the corner    */
float  angleVal1;             /* dfi(X) less than this value is not reduced   */
float  angleVal2;             /* dfi(X) greater than this value is ignored    */
float  sigmaD;                /* sigma for the probability density pd()       */
float  sigmaR;                /* sigma for the weight function wr()           */
float  invAngleRampWidth;     /* a precomputed value to save time             */
float *gSizes;                /* sizes of the gradient of brightness, g(X)    */
float *dets;                  /* values of Beaudet's Det prop. to curvature   */
float *gDirs;                 /* directions of the gradient, fi(X)            */
short *gDsins;                /* sins of fi(X) to speed up the computation    */
short *gDcoss;                /* coss of fi(X) for the same reason            */
float *corrDirs;              /* directions of corners                        */
float *corrs;                 /* corner measures, corr(Q)                     */
float *appars;                /* corner apparences, appar(Q)                  */
float *SNRs;                  /* Signal-To-Noise ratio for corner candidates  */
int   *neighbMap;             /* maps neighbourhood of Q into all image files */
float *Psgs;                  /* values of Psg(X) for Q just being processed  */
float *Wrs;                   /* the values of the weight Wr(x,y)             */
float *PsgWrs;                /* the values of PsgWr(X)                       */
int   *xs,*ys;                /* x and y coordinates of the points in the mask*/
float *dirXs;                 /* directions towards the points in the mask    */
float *diffDirXs;             /* differences between fi(Q) and fi(X)          */
int   *flagsProcess;          /* flags "the point will be processed"          */
int   *influenceMap;          /* map of points influenced by Psg(X)           */
int   *influences;            /* number and indices of influenced points      */
float *intCoefs;              /* coefficients for interpolating Psg(X)        */
int   *importantPixs;         /* list of pixels relevant for measuring at Q   */
int    numImportantPixs;      /* number of the pixels in the above list       */
int   *tobeProcessed;         /* influenced points waiting for processing     */
float *pdTable;               /* the values of probability density pd()       */
int    pdTableSize;           /* the size of the table stated above           */
int    pdTableScale;          /* values are tabulated with step 1/pdTableScale*/
float  pdTableScalef;         /* the same but represented as a float          */
float  pdTableScale2f;        /* the value stated above multiplied by 2.0     */
short *isCandidate;           /* contains 1 at points where corr() has maxima */
int    halfExtMaskSize;       /* in the neighbourhood of this size, the maxima*/
                              /* of coors(Q) will be checked                  */
int   *corners;               /* pointer to array containing the corners      */
int    options;               /* options required for "detectCorners"         */


static inline float mysqrt(float x)
{
	return sqrt(x);
}

/*----------------------------------------------------------------------------*/
/* Computes the neighbMap. Each item of this map incremented by imgPosQ points*/
/* to the appropriate position in the whole image from where the image values */
/* can be read.                                                               */
static
void createNeighbMap(void){
    int x, y, imgPos, n = 0, stride = xImageSize - PsgMaskSize;
    imgPos = -halfPsgMaskSize * xImageSize - halfPsgMaskSize;
    for (y = -halfPsgMaskSize; y <= halfPsgMaskSize; y++, imgPos += stride)
      for (x = -halfPsgMaskSize; x <= halfPsgMaskSize; x++, imgPos++)
        neighbMap[n++] = imgPos;
}
/*----------------------------------------------------------------------------*/
/*Creates the tables xs, ys.                                                  */
static
void createXsYsTables(void){
    int i, j, k = 0;
    for (i = -halfPsgMaskSize; i <= halfPsgMaskSize; i++)
      for (j = -halfPsgMaskSize; j <= halfPsgMaskSize; j++, k++)
        {xs[k] = j; ys[k] = i;}
}
/*----------------------------------------------------------------------------*/
/* Precomputes the table of values of Wr(X) for all X in the mask.            */
static
void createWrTable(void){
    int x, y, sqr_y; 
    float coef = -0.5f/(sigmaR*sigmaR), *tab = Wrs;
    
    for (y = -halfPsgMaskSize; y <= halfPsgMaskSize; y++) 
    {
      sqr_y = y*y;
      for (x = -halfPsgMaskSize; x <= halfPsgMaskSize; x++, tab++)
        *tab = exp(coef*(float)(sqr_y + x*x));
    }
}
/*----------------------------------------------------------------------------*/
/* Computes dirXs table. Each item of this table contains the value of the    */
/* diresction towards the corresponding point in the mask.                    */
static
void createDirXTable(void){
    int x, y, idx = 0;
    float dirX;
    for (y = -halfPsgMaskSize; y <= halfPsgMaskSize; y++)
      for (x = -halfPsgMaskSize; x <= halfPsgMaskSize; x++)
        if ((x != 0)||(y != 0)){
          dirX = atan2(y,x); 
          	
          	if (dirX < 0) 
          		dirX += TWO_PIf;
          
          dirXs[idx++] = dirX;
        
        } else dirXs[idx++] = 0.0f;
}
/*----------------------------------------------------------------------------*/
/* Computes the sizes and the directions of the gradient of brightness.       */
/* Also, precomputes the values of sins and coss of the direction.            */
#define W1 0.10f
#define W2 0.30f
static
void computeGrads(void){
    int i, x, y, imgPos; 
    float *al, *pl, *nl, a, b, dx, dy, r, angle;
    
   /*printf("numImagePixs: %d\n", numImagePixs);*/
  /*  for( i = 0; i < numImagePixs; i++ ) 
    	gSizes[i] = 0.0f;*/
    
    imgPos = xImageSize + 1;  /*  since we skip the first (and the last) line */
    pl = &image[1]; 
    al = &image[xImageSize+1]; 
    nl = &image[2*xImageSize+1];
    
       
    for (y = 1; y < yImageSize - 1; y++, imgPos +=2, pl+=2, al+=2, nl+=2)
      for (x = 1; x < xImageSize - 1; x++, imgPos++, pl++, al++, nl++){
        a = *(nl + 1) - *(pl - 1);
        b = *(nl - 1) - *(pl + 1);
        
        dx = W1*(a - b) + W2*(*(al + 1) - *(al - 1));
        dy = W1*(a + b) + W2*(     *nl  -      *pl );
        if ((r = dx*dx + dy*dy) > 0.001f ){
          angle = atan2f(dy,dx); if (angle < 0) angle += TWO_PIf;
          gSizes[imgPos] = r = sqrt(r);
          gDirs[imgPos]  = angle;
          gDsins[imgPos] = (short)(pdTableScalef*dy/r);
          gDcoss[imgPos] = (short)(pdTableScalef*dx/r);
        }
      }
}
/*----------------------------------------------------------------------------*/
/* Computes the square of Beaudet's DET that is used for estimating the noise.*/
static
void computeDets(void){
    int i, x, y, imgPos; 
    float *al, *pl, *nl;
    float a11,a12,a13, a21,a22,a23, a31,a32,a33;
    float r,s,t, det;
    
    /*for( i = 0; i < numImagePixs; i++ ) 
    	dets[i] = 0.0f;*/
    
    imgPos = xImageSize + 1;   /* since we skip the first (and the last) line */
    pl = &image[1]; 
    al = &image[xImageSize+1]; 
    nl = &image[2*xImageSize+1];
    
    for (y = 1; y < yImageSize - 1; y++, imgPos +=2, pl+=2, al+=2, nl+=2)
      for (x = 1; x < xImageSize - 1; x++, imgPos++, pl++, al++, nl++){
        if (gSizes[imgPos] <= noiseGradSizeThresh){
          a31 = *(nl-1); a32 = *nl; a33 = *(nl+1);
          a21 = *(al-1); a22 = *al; a23 = *(al+1);
          a11 = *(pl-1); a12 = *pl; a13 = *(pl+1);
          r = a21 - 2.0f*a22 + a23;
          s = 0.25f*(a11 + a33 - a31 - a13);
          t = a12 - 2.0f*a22 + a32;
          det = r*t - s*s;
          dets[imgPos] = det * det;
        }
      }
}

static
float estimateNoiseSigma(int imgPosQ){
    int   x, y, mapIdx = 0, cnt = 0, imgPos;
    float sum = 0.0f, meanDet, sigma, c = 8.0f;
    for (y = -halfPsgMaskSize; y <= halfPsgMaskSize; y++)
      for (x = -halfPsgMaskSize; x <= halfPsgMaskSize; x++){
        imgPos = imgPosQ + neighbMap[mapIdx++];
        if (gSizes[imgPos] <= noiseGradSizeThresh){sum += dets[imgPos]; cnt++;}
      }
   if (cnt) {
     meanDet = sqrt( sum / (float)(cnt));
     sigma = sqrt(meanDet / c);
     if (sigma > noiseGradSizeThresh) sigma = meanDet/(c * noiseGradSizeThresh);
     return sigma;
   } else return 0.0f;
}
/*----------------------------------------------------------------------------*/
/* Precomputes the table of values of the probability density pd(z).          */
static
void createPdTable(void){
    int i; float *tab = pdTable;
    float coef = -0.5f/((pdTableScalef*pdTableScalef)*(sigmaD*sigmaD));
    
    printf("pdTableSize :%d\n", pdTableSize);
    
    for (i = 0; i < pdTableSize; i++, tab++) 
    	*tab = expf(coef*(float)(i*i));
}
/*----------------------------------------------------------------------------*/
/* Precomputes the tables for organising computation and interpolating Psg(X).*/

#define getMaskIdx(x,y) \
k = ((y) + halfPsgMaskSize)*PsgMaskSize + (x) + halfPsgMaskSize;

#define send3(x,y,s1,s2,s3) \
  k = ((y) + halfPsgMaskSize)*PsgMaskSize + (x) + halfPsgMaskSize;\
  influenceMap[k] = q;     influences[q++] = 3;\
  influences[q] = k + s1;  intCoefs[q++] = c;\
  influences[q] = k + s2;  intCoefs[q++] = c;\
  influences[q] = k + s3;  intCoefs[q++] = 1.0f;\

#define send2(x,y,s1,s2) \
  k = ((y) + halfPsgMaskSize)*PsgMaskSize + (x) + halfPsgMaskSize;\
  influenceMap[k] = q;     influences[q++] = 2;\
  influences[q] = k + s1;  intCoefs[q++] = c1;\
  influences[q] = k + s2;  intCoefs[q++] = c2;\

static
void createOrgTables(int halfMaskSize){
    const int numMaskPixels = PsgMaskSize * PsgMaskSize,
      E =  1, NE =  1 - PsgMaskSize, N = -PsgMaskSize, NW = -1 - PsgMaskSize,
      W = -1, SW =  PsgMaskSize - 1, S =  PsgMaskSize, SE =  PsgMaskSize + 1;
    int i, j, k, q = 0; float c, c1, c2;

    for (k = 0; k < numMaskPixels; k++) flagsProcess[k] = 0;
    for (k = 0; k < numMaskPixels; k++) influenceMap[k] = 0;

    influences[q++] = 0;

    for (i = 1; i < halfMaskSize; i++){
      c = 1.0f /(float)(i+1);
      send3( i, 0, SE, NE,  E);
      send3( i,-i,  E,  N, NE);
      send3( 0,-i, NE, NW,  N);
      send3(-i,-i,  N,  W, NW);
      send3(-i, 0, NW, SW,  W);
      send3(-i, i,  W,  S, SW);
      send3( 0, i, SW, SE,  S);
      send3( i, i,  S,  E, SE);
      for (j = 1; j < i; j++){
        c1 = c*(float)(j+1); c2 = c*(float)(i-j+1);
        send2( i, j, SE, E);
        send2( i,-j, NE, E);
        send2( j,-i, NE, N);
        send2(-j,-i, NW, N);
        send2(-i,-j, NW, W);
        send2(-i, j, SW, W);
        send2(-j, i, SW, S);
        send2( j, i, SE, S);
      }
    }
}
/*----------------------------------------------------------------------------*/
/* Computes the values of Psg(X) for a point Q that is just being processed.  */
/* prejst createPsgMap .. */
#define diffAngles(a1,a2,d) d = (a1) - (a2);\
if (d > PIf) d -= TWO_PIf; else if (d < -PIf) d += TWO_PIf;\

static 
int createPsgMap(int imgPosQ){

    float bQ, gQ, estCorrDir, diffDirX, absDiffDirX, PsgX, newPsgX;
    int   mapPosX, imgPosX, dX, hX, numInfluences, influencePosX, k;
    int   idxProcess = 0, idxInsert = 0, pixCount = 0;

    bQ  = image[imgPosQ]; 
    gQ  = gSizes[imgPosQ];

    
    diffDirX = 0;
    diffDirXs[PsgMaskCenter] = diffDirX;
    Psgs[PsgMaskCenter] = PsgWrs[PsgMaskCenter] = 1.0f;
    
    
    corrDirs[imgPosQ] = estCorrDir = gDirs[imgPosQ];
    
    importantPixs[pixCount++] = PsgMaskCenter;

    Psgs[(tobeProcessed[idxInsert++] = PsgMaskCenter + 1)] = 1.0f;
    Psgs[(tobeProcessed[idxInsert++] = PsgMaskCenter - 1)] = 1.0f;
    k = PsgMaskCenter - PsgMaskSize;
    Psgs[(tobeProcessed[idxInsert++] = k - 1)] = 1.0f;
    Psgs[(tobeProcessed[idxInsert++] = k    )] = 1.0f;
    Psgs[(tobeProcessed[idxInsert++] = k + 1)] = 1.0f;
    k = PsgMaskCenter + PsgMaskSize;
    Psgs[(tobeProcessed[idxInsert++] = k - 1)] = 1.0f;
    Psgs[(tobeProcessed[idxInsert++] = k    )] = 1.0f;
    Psgs[(tobeProcessed[idxInsert++] = k + 1)] = 1.0f;

    while (idxProcess < idxInsert){
      mapPosX = tobeProcessed[idxProcess++];
      flagsProcess[mapPosX] = 0;
      if ((PsgX = Psgs[mapPosX]) > SMALL_PROBABILITY){
        if (gSizes[imgPosX = imgPosQ+neighbMap[mapPosX]] > noiseGradSizeThresh){
          diffAngles(gDirs[imgPosX], estCorrDir, diffDirX);
          
          if (diffDirX >= 0.0f) absDiffDirX = diffDirX;
          else absDiffDirX = -diffDirX;
          
          if (absDiffDirX < angleVal2){
            hX = abs(xs[mapPosX]*gDcoss[imgPosX] + ys[mapPosX]*gDsins[imgPosX]);
            printf("hX: %d \n", hX);
            if (hX < pdTableSize){
              dX=(int)(pdTableScale2f*(image[imgPosX]-bQ)/(gSizes[imgPosX]+gQ));
              if (dX < 0) dX = - dX;
              if (dX < pdTableSize){
                newPsgX = pdTable[dX]*pdTable[hX];
                if (absDiffDirX > angleVal1)
                  newPsgX *= 1.0f - (absDiffDirX - angleVal1)*invAngleRampWidth;
                if (newPsgX > SMALL_PROBABILITY){
                  if (newPsgX < PsgX) PsgX = newPsgX;
                  k = influenceMap[mapPosX];
                  if ((numInfluences = influences[k]) > 0){
                    
                	if (!flagsProcess[influencePosX = influences[++k]])
                	{
                      Psgs[influencePosX] = intCoefs[k]*PsgX;
                      tobeProcessed[idxInsert++] = influencePosX;
                      flagsProcess[influencePosX] = 1;
                    } 
                		else Psgs[influencePosX] += intCoefs[k]*PsgX;

                	if (!flagsProcess[influencePosX = influences[++k]]){
                      Psgs[influencePosX] = intCoefs[k]*PsgX;
                      tobeProcessed[idxInsert++] = influencePosX;
                      flagsProcess[influencePosX] = 1;
                    } else Psgs[influencePosX] += intCoefs[k]*PsgX;

                	if (numInfluences == 3)
                      Psgs[(tobeProcessed[idxInsert++]=influences[++k])] = PsgX;
                  }
                  diffDirXs[mapPosX] = diffDirX;
                  Psgs[mapPosX] = PsgX;
                  PsgWrs[mapPosX] = PsgX*Wrs[mapPosX];
                  importantPixs[pixCount++] = mapPosX;
                }
              }
            }
          }
        }
      }
    }
    return pixCount;
}
/*----------------------------------------------------------------------------*/
/* Computes the corner direction, corner angle, corner mean gradient size,    */
/* and the corner area size for the corner candidate at Q.                    */

static
void measureCornerCandidate(int imgPosQ, float *variance,
                            float *apparence, float *corrDir,
                            float *meanGr, float *inertiaRadius){

    float sum1, sum1w, sum2, sum2w, sum3, diffDir, dir, dir1, dir2, cDir;
    float halfAngle, w, s, c, d, refAngleVal = halfAngleThresh/2.5f;
    int   i, j;

   *apparence = MIN_APPAR; *meanGr = *inertiaRadius = 0.0f;
    sum1 = sum1w = sum2 = 0.0f;
    for (i = 0; i < numImportantPixs; i++){
      j = importantPixs[i]; w = PsgWrs[j];
      sum1w += w; sum1 += w*diffDirXs[j];
    }
    dir = sum1/sum1w;

    cDir = dir + corrDirs[imgPosQ];
    if (cDir > TWO_PIf) cDir -= TWO_PIf; else if(cDir < 0.0f ) cDir += TWO_PIf;
    *corrDir = cDir;

    for (i = 0; i < numImportantPixs; i++){
      j = importantPixs[i];
      diffDir = diffDirXs[j] - dir; if (diffDir < 0.0f) diffDir = -diffDir;
      sum2 += diffDir*diffDir*PsgWrs[j];
    }
    *variance  = sum2/sum1w; halfAngle = sqrt(*variance);
    if (halfAngle < halfAngleThresh) return;

    if (halfAngle < 0.4f){
      s = sin(cDir); c = cos(cDir);
      sum1 = sum1w = sum2 = sum2w = 0.0f;
      for (i = 0; i < numImportantPixs; i++){
        j = importantPixs[i]; w = Psgs[j];
        d = -s*(float)(xs[j]) + c*(float)(ys[j]);
        if (d > 0) {sum1w += w; sum1 += w*diffDirXs[j];}
        if (d < 0) {sum2w += w; sum2 += w*diffDirXs[j];}
      }

      if (sum1w > 0.0f) dir1 = sum1/sum1w; else dir1 = 0.0f;
      if (sum2w > 0.0f) dir2 = sum2/sum2w; else dir2 = 0.0f;

      sum3 = 0.0f;
      for (i = 0; i < numImportantPixs; i++){
        j = importantPixs[i]; w = Psgs[j];
        d = -s*(float)(xs[j]) + c*(float)(ys[j]);
        if (d > 0){diffDir = diffDirXs[j] - dir1; sum3 += w*diffDir*diffDir;}
        if (d < 0){diffDir = diffDirXs[j] - dir2; sum3 += w*diffDir*diffDir;}
      }
      refAngleVal = 0.7f*sqrt(sum3/(sum1w + sum2w));
    }

    sum1 = sum1w = sum2 = sum2w = sum3 = 0.0f;
    for (i = 0; i < numImportantPixs; i++){
      j = importantPixs[i];
      diffDir = diffDirXs[j] - dir; if (diffDir < 0.0f) diffDir = -diffDir;
      if (diffDir > refAngleVal){
        w = gSizes[imgPosQ + neighbMap[j]] * Psgs[j];
        sum1w += w; sum1 += w * (xs[j]*xs[j] + ys[j]*ys[j]);
        w = PsgWrs[j];
        sum2w += w; sum2 += w * gSizes[imgPosQ + neighbMap[j]];
      }
      sum3  += (diffDir - refAngleVal)*gSizes[imgPosQ + neighbMap[j]]*Psgs[j];
    }
    *apparence = sum3;
    if (sum1w > 0.0f){*inertiaRadius = sqrt(sum1/sum1w); *meanGr = sum2/sum2w;}
}
/*----------------------------------------------------------------------------*/
/* This function computes the values of corr(Q) and appar(Q) for all the image*/
/* points (except the points in the border of width halfMaskSize + 1).        */

static
void computeMeasuresAndApparences(void){
    const int first  = halfPsgMaskSize + 1, doubleFirst = 2 * first,
              last_x = xImageSize - halfPsgMaskSize - 2,
              last_y = yImageSize - halfPsgMaskSize - 2;
    int       x, y, i, imgPosQ = first * xImageSize + first;
    float     variance, appar, dir, meanGr, inertiaRadius;

    /*for( i = 0; i < numImagePixs; i++ )  corrs[i] = 0.0f;*/
    for( i = 0; i < numImagePixs; i++ ) appars[i] = MIN_APPAR;
    for (y = first; y <= last_y; y++, imgPosQ += doubleFirst)
      for (x = first; x <= last_x; x++, imgPosQ++)
        if (gSizes[imgPosQ] > noiseGradSizeThresh){
          numImportantPixs = createPsgMap(imgPosQ);
          if (numImportantPixs >= SMALL_NUM_PIXELS){

            measureCornerCandidate(imgPosQ, &variance, &appar,
                                   &dir, &meanGr, &inertiaRadius);

            if(inertiaRadius  < inertiaRadiusThresh) appar = MIN_APPAR;
            if(meanGr         <  meanGradSizeThresh) appar = MIN_APPAR;
            corrs[imgPosQ]    = variance * gSizes[imgPosQ];
            appars[imgPosQ]   = appar;
            corrDirs[imgPosQ] = dir;
            SNRs[imgPosQ]     = meanGr;
          }
        }
}
/*----------------------------------------------------------------------------*/
/* This function decides whether the function corrs has its maximum at Q.     */
static
short isCorrMax(int imgPosQ){
    float corr = corrs[imgPosQ];
    if ((corr >= corrs[imgPosQ + 1])&&
        (corr >= corrs[imgPosQ - xImageSize + 1])&&
        (corr >= corrs[imgPosQ - xImageSize])&&
        (corr >= corrs[imgPosQ - xImageSize - 1])&&
        (corr >= corrs[imgPosQ - 1])&&
        (corr >= corrs[imgPosQ + xImageSize - 1])&&
        (corr >= corrs[imgPosQ + xImageSize])&&
        (corr >= corrs[imgPosQ + xImageSize + 1]))
          return 1;
    else  return 0;
}
/*----------------------------------------------------------------------------*/
/* The function markCandidates selects the points at which corr() has its     */
/* and apparency is greater than zero.                                        */
static
void markCandidates(void){
    const int first  = halfPsgMaskSize + 1, doubleFirst = 2*first,
              last_x = xImageSize - halfPsgMaskSize - 2,
              last_y = yImageSize - halfPsgMaskSize - 2;
    int       x, y, imgPosQ;

    imgPosQ = first * xImageSize + first;
    for (y = first; y <= last_y; y++, imgPosQ += doubleFirst)
      for (x = first; x <= last_x; x++, imgPosQ++)
        if (appars[imgPosQ] > MIN_APPAR){
          isCandidate[imgPosQ] = isCorrMax(imgPosQ);
        } else isCandidate[imgPosQ] = 0;
}
/*----------------------------------------------------------------------------*/
/* The function modifyApparency that follows modifies the value of appars(Q)  */
/* by multiplying it by a coefficient that may decrease the value of apprence */
/* of corner at Q. The coefficient is determined by examining the level of    */
/* noise of the size of the gradient of brigtness in the neighbourhood of Q.  */
static
void modifyApparency(void){
    const int first  = halfPsgMaskSize + 1, doubleFirst = 2*first,
              last_x = xImageSize - halfPsgMaskSize - 2,
              last_y = yImageSize - halfPsgMaskSize - 2;
    float     meanSNR, coef, logMeanSNR, sum = 0.0;
    int       x, y, imgPosQ, cnt = 0;

    imgPosQ = first * xImageSize + first;
    for (y = first; y <= last_y; y++, imgPosQ += doubleFirst)
      for (x = first; x <= last_x; x++, imgPosQ++)
        if (isCandidate[imgPosQ]){
          SNRs[imgPosQ] /= (estimateNoiseSigma(imgPosQ) + 0.01f*SNRs[imgPosQ]);
          sum += SNRs[imgPosQ]; cnt++;
        }

    if (!cnt) return;
    meanSNR = sum / (float)(cnt);
    logMeanSNR = log(meanSNR);

    imgPosQ = first * xImageSize + first;
    for (y = first; y <= last_y; y++, imgPosQ += doubleFirst)
      for (x = first; x <= last_x; x++, imgPosQ++)
        if (isCandidate[imgPosQ]){
          coef = log(SNRs[imgPosQ]) / logMeanSNR;
          appars[imgPosQ] *= coef;
        }
}
/*----------------------------------------------------------------------------*/
/* This function checkes whether an another greater maximum of corrs() exists */
/* in the neighbourhood of Q (imgPosQ) whose radius is halfExtNeighbSize. If  */
/* so, if the corner directions are similar at both the maxima, and if both   */
/* the maxima are not separated by a low value of corrs() between them, the   */
/* candidate will be rejected as a corner.                                    */

static
int checkCornerInGreaterNeighbourhood(int imgPosQ){
    int   mapPosX, imgPosX, idxProcess = 0, idxInsert = 0;
    int   numInfluences, influencePosX, k;
    float diffDirX, corrX, minSepX, *minSep = Psgs,
          corrQ = corrs[imgPosQ], corrDirQ = corrDirs[imgPosQ],
          halfAngleQ = (HALF_PIf - sqrt(corrQ/gSizes[imgPosQ]));

    minSep[(tobeProcessed[idxInsert++] = PsgMaskCenter + 1)] = corrQ;
    minSep[(tobeProcessed[idxInsert++] = PsgMaskCenter - 1)] = corrQ;
    k = PsgMaskCenter - PsgMaskSize;
    minSep[(tobeProcessed[idxInsert++] = k - 1)] = corrQ;
    minSep[(tobeProcessed[idxInsert++] = k    )] = corrQ;
    minSep[(tobeProcessed[idxInsert++] = k + 1)] = corrQ;
    k = PsgMaskCenter + PsgMaskSize;
    minSep[(tobeProcessed[idxInsert++] = k - 1)] = corrQ;
    minSep[(tobeProcessed[idxInsert++] = k    )] = corrQ;
    minSep[(tobeProcessed[idxInsert++] = k + 1)] = corrQ;

    while (idxProcess < idxInsert){
      mapPosX = tobeProcessed[idxProcess++];
      imgPosX = imgPosQ + neighbMap[mapPosX];
      flagsProcess[mapPosX] = 0;
      minSepX = minSep[mapPosX];
      corrX   = corrs[imgPosX];

      if (corrX > corrQ){
        if (isCandidate[imgPosX]){
          diffAngles(corrDirQ, corrDirs[imgPosX], diffDirX);
          if (diffDirX < 0.0f) diffDirX = -diffDirX;
          if (diffDirX < halfAngleQ){
            diffAngles(corrDirQ, dirXs[mapPosX], diffDirX);
            if (diffDirX < 0.0f) diffDirX = -diffDirX;
            if ((PIf - diffDirX) < diffDirX) diffDirX = PIf - diffDirX;
            if (diffDirX < halfAngleQ){
              if ((minSepX*corrX)/(corrQ*corrQ) > 0.25f){
                while (idxProcess < idxInsert)
                  flagsProcess[tobeProcessed[idxProcess++]] = 0;
                return 0;
              }
            }
          }
        }
      }

      k = influenceMap[mapPosX];
      if ((numInfluences = influences[k]) > 0){
        if(corrX > 0.0f){
          diffAngles(corrDirQ, corrDirs[imgPosX], diffDirX);
          if (diffDirX < 0.0f) diffDirX = -diffDirX;
          if (diffDirX > halfAngleQ) corrX = 0.0f;
          if (corrX < minSepX) minSepX = corrX;
        } else minSepX = 0.0f;

        if (!flagsProcess[influencePosX = influences[++k]]){
          minSep[influencePosX] = intCoefs[k]*minSepX;
          tobeProcessed[idxInsert++] = influencePosX;
          flagsProcess[influencePosX] = 1;
        } else minSep[influencePosX] += intCoefs[k]*minSepX;
        if (!flagsProcess[influencePosX = influences[++k]]){
          minSep[influencePosX] = intCoefs[k]*minSepX;
          tobeProcessed[idxInsert++] = influencePosX;
          flagsProcess[influencePosX] = 1;
        } else minSep[influencePosX] += intCoefs[k]*minSepX;
        if (numInfluences == 3)
          minSep[(tobeProcessed[idxInsert++]=influences[++k])] = minSepX;
      }
    }
    return 1;
}
/*----------------------------------------------------------------------------*/
/* The function decideCorners goes through the values of corr(Q) and appar(Q) */
/* and searches for the corners. A point Q is marked as a corner if corr(Q)   */
/* has its maximum at Q and if appar(Q) is greater than a predefined          */
/* threshold. The coordinates x,y of the corner points are written into       */
/* the array "cornerList". The function returns the number of corners that    */
/* were found. It returns -2 if the array is not big enough.                  */

#define addToCornerList(x,y) {\
    if (cornerListPos > (cornerListLen - 2)) return -2;\
    cornerList[cornerListPos++] = x; cornerList[cornerListPos++] = y; }

static
int decideCorners(int *cornerList, const int cornerListLen){
	const int first  = halfPsgMaskSize + 1, doubleFirst = 2*first,
	last_x = xImageSize - halfPsgMaskSize - 2,
	last_y = yImageSize - halfPsgMaskSize - 2;
	int       x, y, imgPosQ, cornerListPos = 0;

	markCandidates();
	if (options & CHECK_APPAR_SNR) modifyApparency();

	if (halfExtMaskSize == 1)
	{
		imgPosQ = first * xImageSize + first;
		for (y = first; y <= last_y; y++, imgPosQ += doubleFirst)
			for (x = first; x <= last_x; x++, imgPosQ++)
				if ((isCandidate[imgPosQ])&&(appars[imgPosQ] > apparenceThresh))
					addToCornerList(x,y);
	} 
	else 
	{
		imgPosQ = first * xImageSize + first;
		for (y = first; y <= last_y; y++, imgPosQ += doubleFirst)
			for (x = first; x <= last_x; x++, imgPosQ++)
				if ((isCandidate[imgPosQ])&&(appars[imgPosQ] > apparenceThresh)){
					if (checkCornerInGreaterNeighbourhood(imgPosQ))
						addToCornerList(x,y);
				}
	}
	return cornerListPos / 2;
}
/*----------------------------------------------------------------------------*/
/* This function sorts the corners according to their apparence (obviousness).*/
static
void sortCorners( int l, int r ){
    int i, j, c, tmp; float val;
      i = l; j = r;
      c = (l+r)/2; if (c & 1) c--;
      val = appars[corners[c] + corners[c+1]*xImageSize];
      do {
        while (appars[corners[i] + corners[i+1]*xImageSize] > val){i+=2;}
        while (val > appars[corners[j] + corners[j+1]*xImageSize]){j-=2;}
        if (i <= j){
          tmp= corners[i];   
          corners[i]  = corners[j];   
          corners[j]  = tmp;
          
          tmp= corners[i+1]; 
          corners[i+1]= corners[j+1]; 
          corners[j+1]= tmp;
          
          i+=2; 
          
          j-=2;
        }
      } while (i <= j);
      if (l < j) sortCorners( l, j );
      if (i < r) sortCorners( i, r );
}
/*============================================================================*/
/* This is the "main function" that performs the computation. If you want to  */
/* use the algorithm, you will only call this function. You will find the     */
/* detailed explanation of its parameters as well as practical advice how     */
/* to set them in the file "cordet.h". You can also have a look at the test   */
/* program in "cortest.c" and the corresponding script files to obtain some   */
/* practical examples.                                                        */
/* The function returns either the number of corners that were found or -1    */
/* if there was not enough memory to solve the task or -2 if the array        */
/* "cornerList" was not big enough for all the corners that were found.       */

int detectCorners(
    float *image_p,
    int    xImageSize_p,
    int    yImageSize_p,
    int    halfPsgMaskSize_p,
    float  angleThresh_p,
    float  noiseGradSizeThresh_p,
    float  apparenceThresh_p,
    float  meanGradSizeThresh_p,
    float  inertiaRadiusThresh_p,
    float  sigmaD_p,
    float  sigmaR_p,
    int    halfExtMaskSize_p,
    int    options_p,
    int   *cornerList,
    int    cornerListLen)
{
/*----------------------------------------------------------------------------*/

    int numCorners; float tmp;

    image               = image_p;
    xImageSize          = xImageSize_p;
    yImageSize          = yImageSize_p;
    halfPsgMaskSize     = halfPsgMaskSize_p;
    angleThresh         = angleThresh_p;
    noiseGradSizeThresh = noiseGradSizeThresh_p;
    apparenceThresh     = apparenceThresh_p;
    meanGradSizeThresh  = meanGradSizeThresh_p;
    inertiaRadiusThresh = inertiaRadiusThresh_p;
    sigmaD              = sigmaD_p;
    sigmaR              = sigmaR_p;
    halfExtMaskSize     = halfExtMaskSize_p;
    options             = options_p;
    corners             = cornerList;
    numImagePixs        = xImageSize*yImageSize;
    PsgMaskSize         = 2*halfPsgMaskSize + 1;
    numPsgMaskPixs      = PsgMaskSize * PsgMaskSize;
    PsgMaskCenter       = PsgMaskSize * halfPsgMaskSize + halfPsgMaskSize;
    halfGradSizeThresh  = 0.5f*noiseGradSizeThresh;
    tenthGradSizeThresh = 0.1f*noiseGradSizeThresh;
    apparenceThresh    *= noiseGradSizeThresh;
    halfAngleThresh     = angleThresh / 2.0f;
    angleVal1           = HALF_PIf - ANGLE_RAMP_WIDTH;
    angleVal2           = HALF_PIf + 0.0f;
    invAngleRampWidth   = 1.0f / (angleVal2 - angleVal1);
    pdTableScale        = PD_TABLE_SCALE;
    pdTableScalef       = (float)pdTableScale;
    pdTableScale2f      = 2.0f*pdTableScalef;
    tmp                 = sqrt(-2.0f*sigmaD*sigmaD*log(SMALL_PROBABILITY));
    pdTableSize         = (int)(pdTableScalef*tmp) + 2;

    gSizes              = malloc(numImagePixs * sizeof(gSizes[0]));
    dets                = malloc(numImagePixs * sizeof(dets[0]));
    gDirs               = malloc(numImagePixs * sizeof(gDirs[0]));
    gDsins              = malloc(numImagePixs * sizeof(gDsins[0]));
    gDcoss              = malloc(numImagePixs * sizeof(gDcoss[0]));
    corrDirs            = malloc(numImagePixs * sizeof(corrDirs[0]));
    corrs               = malloc(numImagePixs * sizeof(corrs[0]));
    appars              = malloc(numImagePixs * sizeof(appars[0]));
    SNRs                = malloc(numImagePixs * sizeof(SNRs[0]));
    neighbMap           = malloc(numPsgMaskPixs  * sizeof(neighbMap[0]));
    Psgs                = malloc(numPsgMaskPixs  * sizeof(Psgs[0]));
    Wrs                 = malloc(numPsgMaskPixs  * sizeof(Wrs[0]));
    PsgWrs              = malloc(numPsgMaskPixs  * sizeof(PsgWrs[0]));
    xs                  = malloc(numPsgMaskPixs  * sizeof(xs[0]));
    ys                  = malloc(numPsgMaskPixs  * sizeof(ys[0]));
    dirXs               = malloc(numPsgMaskPixs  * sizeof(dirXs[0]));
    diffDirXs           = malloc(numPsgMaskPixs  * sizeof(diffDirXs[0]));
    flagsProcess        = malloc(numPsgMaskPixs  * sizeof(flagsProcess[0]));
    influenceMap        = malloc(numPsgMaskPixs  * sizeof(influenceMap[0]));
    influences          = malloc((4*numPsgMaskPixs) * sizeof(influences[0]));
    intCoefs            = malloc((4*numPsgMaskPixs) * sizeof(intCoefs[0]));
    tobeProcessed       = malloc(numPsgMaskPixs  * sizeof(tobeProcessed[0]));
    importantPixs       = malloc(numPsgMaskPixs  * sizeof(importantPixs[0]));
    pdTable             = malloc(pdTableSize     * sizeof(pdTable[0]));

    if ((gSizes != NULL)&&(dets != NULL)&&(gDirs != NULL)&&(gDsins != NULL)&&
        (gDcoss != NULL)&&(corrDirs != NULL)&&(corrs != NULL)&&(SNRs != NULL)&&
        (appars != NULL)&&(neighbMap != NULL)&&(Psgs != NULL)&&(Wrs != NULL)&&
        (PsgWrs != NULL)&&(xs != NULL)&&(ys != NULL)&&(dirXs != NULL)&&
        (diffDirXs != NULL)&&(flagsProcess != NULL)&&(influenceMap != NULL)&&
        (influences != NULL)&&(intCoefs != NULL)&&(tobeProcessed != NULL)&&
        (importantPixs != NULL)&&(pdTable != NULL)){

             isCandidate = gDsins;
             createPdTable();
             createWrTable();
             createNeighbMap();
             createXsYsTables();
             createOrgTables(halfPsgMaskSize);
             computeGrads();
             if (options & CHECK_APPAR_SNR) computeDets();
             computeMeasuresAndApparences();
             createOrgTables(halfExtMaskSize);
             createDirXTable();
             numCorners = decideCorners(cornerList, cornerListLen);
             if (options & SORT_CORNERS){
               if (numCorners > 1){
                 sortCorners(0, 2*(numCorners - 1));
               }
             }

    } else numCorners = -1;

    if (gSizes        != NULL) free(gSizes);
    if (dets          != NULL) free(dets);
    if (gDirs         != NULL) free(gDirs);
    if (gDsins        != NULL) free(gDsins);
    if (gDcoss        != NULL) free(gDcoss);
    if (corrDirs      != NULL) free(corrDirs);
    if (corrs         != NULL) free(corrs);
    if (appars        != NULL) free(appars);
    if (SNRs          != NULL) free(SNRs);
    if (neighbMap     != NULL) free(neighbMap);
    if (Psgs          != NULL) free(Psgs);
    if (Wrs           != NULL) free(Wrs);
    if (PsgWrs        != NULL) free(PsgWrs);
    if (xs            != NULL) free(xs);
    if (ys            != NULL) free(ys);
    if (dirXs         != NULL) free(dirXs);
    if (diffDirXs     != NULL) free(diffDirXs);
    if (flagsProcess  != NULL) free(flagsProcess);
    if (influenceMap  != NULL) free(influenceMap);
    if (influences    != NULL) free(influences);
    if (intCoefs      != NULL) free(intCoefs);
    if (tobeProcessed != NULL) free(tobeProcessed);
    if (importantPixs != NULL) free(importantPixs);
    if (pdTable       != NULL) free(pdTable);

    return numCorners;
}
/*============================================================================*/

